#include <jni.h>
#include "MVAnalyzer.h"
#include <stdio.h>
#include <math.h>
#include <float.h>
#include <time.h>
#include <windows.h>

JNIEXPORT void JNICALL Java_MVAnalyzer_sayHello
  (JNIEnv *env, jobject obj)
{
    printf("Hello world!\n");
    return;
}

typedef struct {
	int x;
	int y;
} location;

unsigned int gettime()
{
 return GetTickCount();
}

#define pow(A, B) ((A)*(A))
//#define sqrt(A) (A)

location find_block(location search_block, jbyte *bbuf_curr, jbyte *bbuf_prev, jint img_width, jint img_height, jint search_range, jint block_width, jint block_height)
{
	location found_block;
	double min_diff = DBL_MAX;
	int min_x = 0, min_y = 0;
	int search_y, search_x;
	int x, y;

	found_block.x = search_block.x;
	found_block.y = search_block.y;

	for (search_y= search_block.y - search_range * block_height; search_y < search_block.y
			+ search_range * block_height; search_y += block_height) {
		if (search_y < 0 || search_y >= img_height)
			continue;

		for (search_x = search_block.x - search_range * block_width; search_x < search_block.x
				+ search_range * block_width; search_x += block_width) {
			double diff = 0;
			if (search_x < 0 || search_x >= img_width) {
				continue;
			}

			for (y = 0; y < block_height; y++) {
				for (x = 0; x < block_width; x++) {

					int prev_r, prev_g, prev_b;
					int r, g, b;
					prev_r = (int)(bbuf_prev[ (search_y + y) * img_width + search_x + x ] & 0xff);
					prev_g = (int)(bbuf_prev[ (search_y + y) * img_width + search_x + x + img_height*img_width] & 0xff);
					prev_b = (int)(bbuf_prev[ (search_y + y) * img_width + search_x + x + img_height*img_width*2] & 0xff);

					//printf(" %d, %d, %d\n", prev_r, prev_g, prev_b);

					r = (int)(bbuf_curr[ (search_block.y + y) * img_width + search_block.x + x ] & 0xff);
					g = (int)(bbuf_curr[ (search_block.y + y) * img_width + search_block.x + x + img_height*img_width] & 0xff);
					b = (int)(bbuf_curr[ (search_block.y + y) * img_width + search_block.x + x + img_height*img_width*2] & 0xff);

					diff += sqrt(pow(prev_r - r, 2)
							+ pow(prev_g - g, 2)
							+ pow(prev_b - b, 2));



				}
			}

			//printf("Diff = %lf prev (%d, %d) -> curr (%d,%d)\n", diff, search_x, search_y, search_block.x, search_block.y );

			// if(search_block.x == (288-16) && search_block.y == (208-16))
			// {
			// System.out.println("Searching " + search_x + "," + search_y);
			// System.out.println(diff);
			// }

			if (diff < min_diff) {
				min_x = search_x;
				min_y = search_y;
				min_diff = diff;
			} else if (diff == min_diff) {
				if (sqrt(pow(search_x - search_block.x, 2)
						+ pow(search_y - search_block.y, 2)) <
						    sqrt(pow(min_x - search_block.x, 2)
								+ pow(min_y - search_block.y, 2))) {
					min_x = search_x;
					min_y = search_y;
				}
			}
		}
	}

	if (min_diff > 10.0f) {
		found_block.x = min_x;
		found_block.y = min_y;
	} else {
		found_block.x = -1;
		found_block.y = -1;


	}

	// System.out.println(search_block.x + "," + search_block.y + "->" +
	// min_x + "," + min_y);
	// if(search_block.x == (288 - 16) && search_block.y == (208 - 16))
	// {
	// System.out.println(min_diff);
	// System.out.println("---");
	// }

	//printf("---------------\n");
	return found_block;
}


JNIEXPORT jdouble JNICALL Java_MVAnalyzer_getMovement
(JNIEnv *env, jobject obj, jobject j_prev, jobject j_curr, jint img_width, jint img_height, jint search_range, jint block_width, jint block_height)
{
	jbyte* bbuf_prev;
	jbyte* bbuf_curr;
	jdouble total_movement = 0;
	int y;
	int x;

	//unsigned int time = gettime();

	bbuf_prev = (*env)->GetDirectBufferAddress(env, j_prev);
	bbuf_curr = (*env)->GetDirectBufferAddress(env, j_curr);

	//System.out.println("Calculating amount of motion...");
	for (y = 0; y < img_height; y += block_height) {
		for (x = 0; x < img_width; x += block_width) {
			location this_block;
			location found_location;

			this_block.x = x;
			this_block.y = y;

			found_location = find_block(this_block, bbuf_curr, bbuf_prev, img_width, img_height, search_range, block_width, block_height);
			//System.out.println(x + "," + y + "->" + found_location.x + "," + found_location.y);

			//ArrayList<location> arr = new ArrayList<location>();

			if (found_location.x != -1) {
				//arr.add(this_block);
				//arr.add(found_location);

				total_movement += sqrt(pow(found_location.x - x,
						2) + pow(found_location.y - y, 2));
				//System.out.println(total_movement);
			}

		}
	}

	//printf("Time: %u\n", gettime() - time);

	return total_movement;
}